#include "zbld_c20.h"
#include "../../bsp/modbus_485.h"
#include "../../bsp/base_type.h"

struct zbld_motor zbld_motor_object = {
    .dir = motor_forward,
    .bus_port = 1,
    .sid = 1,
    .rpm = 3000,
};

static int modbus_single_register_ask(char bus_port, char sid, unsigned short addr, short *data, char mode)
{
    int sta = 0;
    uint8_t rec_data[20] = {0};
    uint8_t rec_len = 0;
    MB_data modbus;
    uint8_t try_times = 5;

    modbus.slave_id = sid;
    modbus.mb_fun = mode == 0 ? mb_single_r : mb_single_w;
    modbus.start_addr = addr;
    modbus.wBuf = (uint16_t *)data;
    modbus.rwLen = 1;
    modbus.resBuf = rec_data;
    modbus.resLen = &rec_len;
    while(try_times--) {
        if( 0 == xp_modbus_ask(bus_port, &modbus) ) {
            *data = *(short *)rec_data;
            return 0;
        }
    }

    return -1;
}

static inline xp_bool zbld_single_reg_set(struct zbld_motor *motor_attr, unsigned short addr, short *data)
{
    return (0 == modbus_single_register_ask(motor_attr->bus_port, motor_attr->sid, addr, data, 1)) ? xp_true : xp_false;
}

static inline xp_bool zbld_single_reg_get(struct zbld_motor *motor_attr, unsigned short addr, short *data)
{
    return (0 == modbus_single_register_ask(motor_attr->bus_port, motor_attr->sid, addr, data, 0)) ? xp_true : xp_false;
}

static int zbld_init(xp_motor_t *motor)
{
    // 配置速度，加减速等
    motor->inited = 1;
    return 0;
}

static int zbld_speed_set(xp_motor_t *motor, motor_rpm_t rpm)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    xp_bool ret = zbld_single_reg_set(motor_attr, 0x2001, (short *)(&rpm));
    if (xp_false == ret) {
        //println("zbld motor set speed to %d failed.", rpm);
        return -1;
    }
    motor_attr->rpm = rpm;
    return 0;
}

static int zbld_speed_get(xp_motor_t *motor, motor_rpm_t *rpm)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    xp_bool ret = zbld_single_reg_get(motor_attr, 0x2001, (short *)rpm);
    if (xp_false == ret) {
        //println("zbld motor get speed failed.");
        return -1;
    }
    motor_attr->rpm = *rpm;
    return 0;
}

static int zbld_rotation_start(xp_motor_t *motor);
static int zbld_direction_set(xp_motor_t *motor, motor_direction_t dir)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    if (motor_attr->dir != dir) {
        motor_attr->dir = dir;
        // 如果电机在运行状态
        if (motor_attr->run != 0) {
            zbld_rotation_start(motor);
        }
    }
    return 0;
}

static int zbld_direction_get(xp_motor_t *motor, motor_direction_t *dir)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    *dir = motor_attr->dir;
    return 0;
}

static int zbld_rotation_start(xp_motor_t *motor)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    short data = (motor_attr->dir == motor_forward) ? 1 : 2;
    xp_bool ret = zbld_single_reg_set(motor_attr, 0x2000, &data);
    if (xp_false == ret) {
        return -1;
    }
    motor_attr->run = 1;
    return 0;
}

static int zbld_rotation_stop(xp_motor_t *motor)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    short data = 5;
    xp_bool ret = zbld_single_reg_set(motor_attr, 0x2000, &data);
    if (xp_false == ret) {
        return -1;
    }
    motor_attr->run = 0;
    return 0;
}

static int zbld_emerg_enable(xp_motor_t *motor)
{
    zbld_rotation_stop(motor);
    // IO急停
    return 0;
}

static int zbld_emerg_disable(xp_motor_t *motor)
{
    // IO急停释放
    return 0;
}

static int zbld_error_get(xp_motor_t *motor, motor_error_t *err_code)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    xp_bool ret = zbld_single_reg_get(motor_attr, 0x2102, (short *)err_code);
    if (xp_false == ret) {
        return -1;
    }
    return 0;
}

static int zbld_error_clear(xp_motor_t *motor)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    short data = 7;
    xp_bool ret = zbld_single_reg_set(motor_attr, 0x2000, &data);
    if (xp_false == ret) {
        return -1;
    }
    return 0;
}


static int zbld_null_function(xp_motor_t *motor, int a, int b)
{
    return 0;
}

int zbld_current_get(xp_motor_t *motor, short *current)
{
    struct zbld_motor *motor_attr = (struct zbld_motor *)motor->private_data;
    xp_bool ret = zbld_single_reg_get(motor_attr, 0x3004, current);
    if (xp_false == ret) {
        return -1;
    }
    return 0;
}

static struct xp_motor_stdops zbld_motor_stdops = {
    .init = zbld_init,
    .speed_set = zbld_speed_set,
    .speed_get = zbld_speed_get,
    .direction_set = zbld_direction_set,
    .direction_get = zbld_direction_get,
    .rotation_start = zbld_rotation_start,
    .rotation_stop = zbld_rotation_stop,
    .emerg_enable = zbld_emerg_enable,
    .emerg_disable = zbld_emerg_disable,
    .error_get = zbld_error_get,
    .error_clear = zbld_error_clear,
    .encode_get = zbld_null_function,
    .position_set = zbld_null_function,
};

void zbld_motor_constructor(struct xp_motor *motor, struct zbld_motor *dev, unsigned char *name)
{
    motor->ops = &zbld_motor_stdops;
    motor->private_data = dev;
    strncpy((char *)motor->name, (char *)name, sizeof(motor->name)-1);
}
